Effort limits for Kinematic Trajectory Optimization #22579

RussTedrake · 2025-02-01T19:54:07Z

Resolves #22500.

+@hongkai-dai for feature review, please?

Note: I've included PR #22552 as the first commit here; the request is only to review the second commit.

This change is

hongkai-dai

Reviewed 6 of 7 files at r2.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @RussTedrake)

planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc line 383 at r2 (raw file):

    KinematicTrajectoryOptimization kto(plant->num_positions(),
                                        num_control_points);
    kto.AddDurationConstraint(1, 1);

Would it be better to use a duration not equal to 1? Using duration equal to 1 would not test whether we have re-scaled the velocity/acceleration with duration in our code.

planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc line 394 at r2 (raw file):

  // Variables for the constraint are [duration, control_points]
  VectorXd x(1 + num_control_points);
  const double duration = 1.0;

Ditto.

planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc line 396 at r2 (raw file):

  const double duration = 1.0;
  auto vdot_ulimit = [&](double q, double v) {
    // ml^2vdot + b*v + mglsin(q) <= 1.0

The right hand side is tau_ub instead of 1.0?

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 200 at r2 (raw file):

 public:
  EffortConstraint(const std::shared_ptr<MultibodyPlant<AutoDiffXd>>& plant,
                   std::unique_ptr<systems::Context<AutoDiffXd>> plant_context,

Curious why we use a unique ptr of plant_context here. Do you think we will have other constraints in the future, that will also use the plant_context that evaluate at s? If so, maybe it is better to share the plant_context pointer to reuse the cache?

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 210 at r2 (raw file):

    // Note: consistency checks on the input arguments already happened in
    // AddEffortBoundsAtNormalizedTimes().
    M_q_ = bspline.EvaluateLinearInControlPoints(s, 0).cast<AutoDiffXd>();

When I first saw the variable name M_vdot_, I thought it means the inertial matrix M times vdot.

Maybe add documentation to clarify it

M_q_, M_v_, M_vdot_ are matrices such that by multiplying control_points with these matrics, we get the position q, the derivative of q w.r.t s, and the second derivative of q w.r.t s respectively.

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 228 at r2 (raw file):

    MatrixX<AutoDiffXd> control_points =
        x.tail(plant_->num_positions() * num_control_points_)
            .reshaped(plant_->num_positions(), num_control_points_);

Do we need to set the time of the plant_context_ to be s * duration

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 576 at r2 (raw file):

  DRAKE_THROW_UNLESS(effort_lb.size() == num_positions());
  DRAKE_THROW_UNLESS(effort_ub.size() == num_positions());
  DRAKE_THROW_UNLESS((effort_lb.array() <= effort_ub.array()).all());

I thought we want to check if B * effort_lb <= B * effort_ub?

RussTedrake

Reviewable status: 7 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @RussTedrake)

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 200 at r2 (raw file):